/**
* @file        DREngine
* @brief       融合惯导的入口类
* @version     1.0.0
* @author      Changlin.Jing,chi.li
* @date        2023/2/16 9:29
* @copyright   Copyright  2023 Langge Software Co., Ltd. All rights reserved.
*/

#include "VDREngine.hpp"
#include <vector>
#include "ut_log_macros.h"
#include "algorithm/vdr_config.h"
#include "../algorithm/TransformCoordinate.hpp"
#include "ut_algorithm.h"
#include "../algorithm/NaviConstant.h"
#include "../algorithm/NaviAttitudeTools.hpp"

/**
 * DR 初始化
 * 初始化信号处理链
 * DR Engine -> Del_Manager->Del_builder->Del_Factory
 * @param workState
 */
bool ILocationObserver::IsNeedUpdate(uint64_t ts, bool IsRawGps) {
    return false;
}

void ILocationObserver::SetOnceLocation(bool isOnceLocation) {

}

void ILocationObserver::SetInterval(std::int32_t interval) {

}
vdr::DREngine *vdr::DREngine::mInstance = nullptr;

vdr::DREngine *vdr::DREngine::GetInstance() {
    if (nullptr == mInstance) {
        mInstance = new DREngine;
    }
    return mInstance;}

void vdr::DREngine::Init(const WorkState &workState) {

}

void vdr::DREngine::Stop() {

}

bool vdr::DREngine::Reset() {
    return false;
}

void vdr::DREngine::OnSignal(const std::shared_ptr<I_LOC_SIGNAL> &signal) {

    if(signal->mLocalSignalType != ACC_SIGNAL
        && signal->mLocalSignalType!=GYRO_SIGNAL
        && signal->mLocalSignalType!=POS_SIGNAL
        && signal->mLocalSignalType !=CAR_SIGNAL
        && signal->mLocalSignalType!=SATELLITE_SIGNAL){
        return;
    }
     SensorDataCache::GetInstance()->storeData(signal);
    //以加速度时间为准更新本地时间
    if (signal->mLocalSignalType == ACC_SIGNAL) {
        mCurTimestampMs = signal->mLocalTickTime;
    }
    //记录启动推算时间
    if (m_DrInitTime_ms < 0 && mCurTimestampMs > 0) {
        m_DrInitTime_ms = mCurTimestampMs;
    }
    bool hasReachDeadline = m_DrInitTime_ms > 0 && (signal->mLocalTickTime - m_DrInitTime_ms > 3000);
    if (!hasWorkStateCheckComplete) {
        if (SensorDataCache::GetInstance()->hasSensorWorkStateCheck || hasReachDeadline) {
            hasWorkStateCheckComplete = true;
        }
    }
    if(!hasWorkStateCheckComplete){
        return;
    }
    if(signal->mLocalSignalType == ACC_SIGNAL || signal->mLocalSignalType==GYRO_SIGNAL ){
        auto naviData = Convert2ImuSignal(signal);
        if(naviData == nullptr){
            return;
        }
        mPtrStaticStateDetection->Insert(naviData);
        if (mPtrNaviFusion->initialized()){
            mPtrNaviFusion->Predict(naviData);
        }
    }


    if(signal->mLocalSignalType == POS_SIGNAL){
        std::shared_ptr<LOC_POS_SIGNAL> posSignal = std::static_pointer_cast<LOC_POS_SIGNAL>(signal);
        if(posSignal->mLocType !=LOC_TYPE_GNSS){
            return;
        }
    }

    auto mLastGnssSignal = SensorDataCache::GetInstance()->mLastPosSignal;

    if(!mPtrNaviFusion->initialized() && mPtrStaticStateDetection->IsStatic()) {
        if(mLastGnssSignal->mLocType == LOC_TYPE_GNSS && mLastGnssSignal->mSpeed > 1.0 & mLastGnssSignal->mHeading > 0){
            double vn= mLastGnssSignal->mSpeed * cos(vdr::UTAlgorithm::ToRadians(mLastGnssSignal->mHeading));
            double ve = mLastGnssSignal->mSpeed * sin(vdr::UTAlgorithm::ToRadians(mLastGnssSignal->mHeading));
            double vd= 0;
            auto rollPitch = mPtrStaticStateDetection->GetRollPitch();
            double roll = rollPitch[0];
            double pitch = rollPitch[1];
            double yaw  = mLastGnssSignal->mHeading > 180 ? mLastGnssSignal->mHeading  - 360:mLastGnssSignal->mHeading;
            yaw = yaw * D2R;
            double st = mLastGnssSignal->mLocalTickTime * mTimeScale;
            gtime_t st_gt{long(st), st - long(st)};
            NaviData::NavInfo initNavInfo(st_gt);
            initNavInfo.vel_ = Eigen::Vector3d { vn, ve,vd};
            UtilTools::Attitude::Euler euler{roll,pitch,yaw};
            auto quat = UtilTools::Attitude::Euler2Quaternion(euler);
            std::cout<<"quat="<<std::endl;
            initNavInfo.quat_ = quat;
            initNavInfo.rotation_ = UtilTools::Attitude::Quaternion2RotationMatrix(initNavInfo.quat_);
            initNavInfo.att_ = UtilTools::Attitude::Quaternion2Euler(initNavInfo.quat_);
            initNavInfo.pos_ = Eigen::Vector3d { mLastGnssSignal->mLatitude * D2R,mLastGnssSignal->mLongitude * D2R,mLastGnssSignal->mHeight};
            mPtrNaviFusion->Init(initNavInfo,15,12);
            return;
        }
    }

    if(mPtrNaviFusion->initialized() && signal->mLocalSignalType == POS_SIGNAL && mLastGnssSignal->mLocType == LOC_TYPE_GNSS && mLastGnssSignal->mAccuracyLocHorizoon < 50){
        std::shared_ptr<LOC_POS_SIGNAL> posSignal = std::static_pointer_cast<LOC_POS_SIGNAL>(signal);
        auto naviData = ConvertGnssSig(posSignal);
        mPtrNaviFusion->Correct(naviData);
    }
    if(mPtrNaviFusion->initialized() && signal->mLocalSignalType == CAR_SIGNAL){
        std::shared_ptr<LOC_CAR_SIGNAL> carSignal = std::static_pointer_cast<LOC_CAR_SIGNAL>(signal);
        auto naviData = ConvertCarSig(carSignal);
        mPtrNaviFusion->Correct(naviData);
    }
    if(mPtrNaviFusion->initialized()){
        NaviData::NavInfo pos;
        mPtrNaviFusion->GetPos(pos);
        if(signal->mLocalTickTime - mLastNotifyTimestamp > 1000){
            LocationInfo locInfo;
            locInfo.m_dLon = pos.pos_[1] * R2D;
            locInfo.m_dLat = pos.pos_[0] * R2D;
            locInfo.m_dHeight = pos.pos_[2];
            locInfo.yaw = pos.att_[2] * R2D;

            locInfo.mLocalTickTime = signal->mLocalTickTime;
            auto iter = m_LocInfoObservers.begin();
            for (; iter != m_LocInfoObservers.end(); ++iter) {
                if (*iter != nullptr) {
                    std::cout.precision(10);
                    (*iter)->OnLocationChanged(locInfo);
                    std::cout<<"fus res:"<< locInfo.m_dLon<<","<<locInfo.m_dLat<<std::endl;
                }
            }
            mLastNotifyTimestamp = signal->mLocalTickTime;
        }
        VDR_FMT_LOG_DEBUG("vdr","%.3f,%.6f,%.6f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f",
            pos.time_.time + pos.time_.sec,
            pos.pos_[1]* R2D,
            pos.pos_[0]* R2D,
            pos.pos_[2],
            pos.vel_[0],
            pos.vel_[1],
            pos.vel_[2],
            pos.att_[0]* R2D,
            pos.att_[1]* R2D,
            pos.att_[2]* R2D,
            pos.acce_bias_[0],
            pos.acce_bias_[1],
            pos.acce_bias_[2],
            pos.gyro_bias_[0],
            pos.gyro_bias_[1],
            pos.gyro_bias_[2]
        )
    }
}

std::shared_ptr<NaviData::BaseData> vdr::DREngine::ConvertGnssSig(std::shared_ptr<LOC_POS_SIGNAL> &posSignal) {
    if (posSignal == nullptr) return nullptr;
    NaviData::NavTime timecur;
    NaviData::GnssData::GNSSOBSDetail gnssobs_detail;
    timecur.setNavtime(posSignal->mLocalTickTime * mTimeScale);
    gnssobs_detail.mUtcTickTime = posSignal->mUtcTickTime;
    gnssobs_detail.mLocType = LOC_TYPE(posSignal->mLocType);
    gnssobs_detail.pos_[0] = posSignal->mLatitude * D2R;
    gnssobs_detail.pos_[1] = posSignal->mLongitude * D2R;
    gnssobs_detail.mSpeed = posSignal->mSpeed;
    gnssobs_detail.mHeading = posSignal->mHeading;
    gnssobs_detail.pos_[2] = posSignal->mHeight;
    gnssobs_detail.mAccuracyVelocity = posSignal->mAccuracyVelocity;
    gnssobs_detail.mAccuracyLocHorizon = posSignal->mAccuracyLocHorizoon;
    gnssobs_detail.mAccuracyLocVertical = posSignal->mAccuracyLocVertical;
    gnssobs_detail.mAccuracyHeadingAngular = posSignal->mAccuracyAngularVelocity;
    gnssobs_detail.sv_nums = posSignal->mValidSatCount;
    auto ptr = std::make_shared<NaviData::GnssData>(timecur, gnssobs_detail);
    return ptr;
}

std::shared_ptr<NaviData::BaseData>
vdr::DREngine::ConvertCarSig(const std::shared_ptr<LOC_CAR_SIGNAL> &carSignal) {
    if (carSignal == nullptr) return nullptr;
    NaviData::NavTime timecur;
    timecur.setNavtime(carSignal->mLocalTickTime * mTimeScale);
    NaviData::CANVelocity::CANVelocityDetail can_vel_detail;
    can_vel_detail.velocity = carSignal->mRealSpeed;
    NaviData::CANVelocity::Ptr canVelocity_ptr = std::make_shared<NaviData::CANVelocity>();
    canVelocity_ptr->setCanVelocity(timecur, can_vel_detail);
    return canVelocity_ptr;
}


bool vdr::DREngine::AddLocationObserver(ILocationObserver *pObserver) {
    if (nullptr == pObserver) {
        return false;
    }
    {
        if (m_LocInfoObservers.find(pObserver) != m_LocInfoObservers.end()) {
            return false;
        }
        m_LocInfoObservers.insert(pObserver);
        return true;
    }
}

bool vdr::DREngine::RemoveLocationObserver(ILocationObserver *pObserver) {
    return false;
}


std::shared_ptr<LOC_POS_SIGNAL> vdr::DREngine::GetFusionSignal(uint64_t time) {
    return std::shared_ptr<LOC_POS_SIGNAL>();
}

void vdr::DREngine::SetConfigFilePath(std::string &filePath) {

}

std::shared_ptr<NaviData::ImuData> vdr::DREngine::Convert2ImuSignal(const std::shared_ptr<I_LOC_SIGNAL> &signal) {
    if (signal == nullptr) return nullptr;
    if (signal->mLocalSignalType == LOC_SIGNAL_TYPE::GYRO_SIGNAL) {
        std::shared_ptr<LOC_GYRO_SIGNAL> gyrSignal = std::static_pointer_cast<LOC_GYRO_SIGNAL>(signal);
        double mcu_time = gyrSignal->mLocalTickTime * mTimeScale;
        mCurrGyroSignal = gyrSignal;
        // 以陀螺仪为线，去串加计数据，acc变化相对低频
        if (!mAccSignalPool.empty()) {
            while (!mAccSignalPool.empty()) {
                mCurrAccSignal = mAccSignalPool.front();
                mAccSignalPool.pop_front();
                // 时间接近在10ms之内即进行匹配
                if (fabs(mCurrAccSignal->mLocalTickTime * mTimeScale -  mCurrGyroSignal->mLocalTickTime * mTimeScale) < 0.009) {
                    break;
                }
            }
        }
        NaviData::ImuData::ImuDataDetail cur_imudata_detail;
        //时间间隔使用eventTime计算的相对时间
        if (gyrSignal->mTimeGap > 0 && gyrSignal->mTimeGap < 50) {
            cur_imudata_detail.dt = gyrSignal->mTimeGap * mTimeScale;
        } else if(mPrevGyroSignal!= nullptr){
            cur_imudata_detail.dt = (mCurrGyroSignal->mLocalTickTime - mPrevGyroSignal->mLocalTickTime)* mTimeScale;
        }else{
            cur_imudata_detail.dt = (mCurrGyroSignal->mLocalTickTime)* mTimeScale;
        }
        mPrevGyroSignal = mCurrGyroSignal;

        if (cur_imudata_detail.dt > 10 * mImuMaxInterval) {
            cur_imudata_detail.dt = mImuMaxInterval;
        }

        // 间距小于等于
        if (cur_imudata_detail.dt <= 1e-5) return nullptr;
        // 放弃插值
        cur_imudata_detail.acce_[0] = -1.0 * mCurrAccSignal->mOriginZ;
        cur_imudata_detail.acce_[1] = -1.0 * mCurrAccSignal->mOriginX;
        cur_imudata_detail.acce_[2]  = 1.0 * mCurrAccSignal->mOriginY;
        cur_imudata_detail.gyro_[0] = -1.0 * mCurrGyroSignal->mOriginZ;
        cur_imudata_detail.gyro_[1] = -1.0 * mCurrGyroSignal->mOriginX;
        cur_imudata_detail.gyro_[2] = 1.0 * mCurrGyroSignal->mOriginY;
        for (int i = 0; i < 3; ++i) {
            cur_imudata_detail.dvel[i] = cur_imudata_detail.acce_[i] * cur_imudata_detail.dt;
            cur_imudata_detail.dtheta[i] = cur_imudata_detail.gyro_[i] * cur_imudata_detail.dt;
        }
        cur_imu.setImuData(mcu_time, cur_imudata_detail);
        pre_imu = cur_imu;

        auto ptr = std::make_shared<NaviData::ImuData>(mCurrGyroSignal->mLocalTickTime, cur_imudata_detail);
        return ptr;

    } else if (signal->mLocalSignalType == LOC_SIGNAL_TYPE::ACC_SIGNAL) {
        std::shared_ptr<LOC_ACC_SIGNAL> accSignal = std::static_pointer_cast<LOC_ACC_SIGNAL>(signal);
        mAccSignalPool.push_back(accSignal);
        while (mAccSignalPool.size() > MAX_SIZE_ACCPOOL) {
            mAccSignalPool.pop_front();
        }
    }
    return nullptr;
}

